#include <stdint.h>
#include <stdbool.h>
#include <sched_clock.h>
#include <cpu.h>
#include "2440addr.h"
#include "io.h"
#include "interrupts.h"
#include "printf.h"
#include "timer.h"
#include <div64.h>

#define FCLK 400000000
#define HCLK (FCLK/4)
#define PCLK (HCLK/2)
#define CONFIG_WD_PERIOD    (10 * 1000 * 1000)  /* 10 seconds default*/

#define TCFG0  ((volatile unsigned int *)0x51000000)    //Timer 0 configuration
#define TCFG1  ((volatile unsigned int *)0x51000004)    //Timer 1 configuration
#define TCON   ((volatile unsigned int *)0x51000008)    //Timer control
#define TCNTB0 ((volatile unsigned int *)0x5100000c)    //Timer count buffer 0
#define TCMPB0 ((volatile unsigned int *)0x51000010)    //Timer compare buffer 0
#define TCNTO0 ((volatile unsigned int *)0x51000014)    //Timer count observation 0
#define TCNTB1 ((volatile unsigned int *)0x51000018)    //Timer count buffer 1
#define TCMPB1 ((volatile unsigned int *)0x5100001c)    //Timer compare buffer 1
#define TCNTO1 ((volatile unsigned int *)0x51000020)    //Timer count observation 1
#define TCNTB2 ((volatile unsigned int *)0x51000024)    //Timer count buffer 2
#define TCMPB2 ((volatile unsigned int *)0x51000028)    //Timer compare buffer 2
#define TCNTO2 ((volatile unsigned int *)0x5100002c)    //Timer count observation 2
#define TCNTB3 ((volatile unsigned int *)0x51000030)    //Timer count buffer 3
#define TCMPB3 ((volatile unsigned int *)0x51000034)    //Timer compare buffer 3
#define TCNTO3 ((volatile unsigned int *)0x51000038)    //Timer count observation 3
#define TCNTB4 ((volatile unsigned int *)0x5100003c)    //Timer count buffer 4
#define TCNTO4 ((volatile unsigned int *)0x51000040)    //Timer count observation 4

#define TIMER_0 (0)
#define TIMER_1 (1)
#define TIMER_2 (2)
#define TIMER_3 (3)
#define TIMER_4 (4)

#define DIVISOR_2  (0)
#define DIVISOR_4  (1)
#define DIVISOR_8  (2)
#define DIVISOR_16 (3)

#define PRESCALER1_SHIFT (8)
#define PRESCALER1_MASK (0xff << PRESCALER1_SHIFT)
#define TIMER2_DIVISOR_SHIFT (8)
#define TIMER2_DIVISOR_MASK (0xf << TIMER2_DIVISOR_SHIFT)

#define TCON_START(chan)        (1 << (4 * (chan ? (chan + 1) : chan) + 0))
#define TCON_MANUALUPDATE(chan) (1 << (4 * (chan ? (chan + 1) : chan) + 1))
#define TCON_INVERT(chan)       (1 << (4 * (chan ? (chan + 1) : chan) + 2))
#define TCON_AUTORELOAD(chan)   (1 << (4 * (chan ? (chan + 1) : chan) + 3))

static unsigned long sched_clock_read_timer0_cyc(void)
{
    uint16_t cyc = io_raw_readw(TCNTO0);
    unsigned long cyc_copy = cyc;
    return ~cyc_copy;
}
static void sched_clock_timer0_free_running(void)
{
    uint32_t tcfg0 = 0, tcfg1 = 0, tcon = 0;

    tcfg0 = io_raw_readl(TCFG0);
    tcfg0 = ((tcfg0 & ~0xff) | 249); //prescaler 249 + 1
    io_raw_writel(tcfg0, TCFG0);
    vprintk("TCFG0:0x%x\n", io_raw_readl(TCFG0));

    tcfg1 = io_raw_readl(TCFG1);
    tcfg1 = ((tcfg1 & ~0xf) | DIVISOR_8); //8 div
    io_raw_writel(tcfg1, TCFG1);
    vprintk("TCFG1:0x%x\n", io_raw_readl(TCFG1));

    io_raw_writew(0xFFFF, TCNTB0);
    io_raw_writew(0xFFFF, TCMPB0);
    vprintk("TCNTB0:%d\n", io_raw_readw(TCNTB0));
    vprintk("TCMPB0:%d\n", io_raw_readw(TCMPB0));

    tcon = io_raw_readl(TCON);
    tcon |= (1 << 1);/* manual update of timer 1 */
    io_raw_writel(tcon, TCON);
    tcon = io_raw_readl(TCON);
    tcon = tcon & (~0xf);
    tcon |= (0x9);
    io_raw_writel(tcon, TCON);/* auto load, start timer */
    vprintk("TCON:0x%x\n", io_raw_readl(TCON));
}

static void sched_clock_timer1_set_prescale_divisor(void)
{
    uint32_t tcfg0 = 0, tcfg1 = 0;

    tcfg0 = io_raw_readl(TCFG0);
    tcfg0 = ((tcfg0 & ~0xff) | 249); //prescaler 249 + 1
    io_raw_writel(tcfg0, TCFG0);
    vprintk("TCFG0:0x%x\n", io_raw_readl(TCFG0));
    tcfg1 = io_raw_readl(TCFG1);
    tcfg1 = ((tcfg1 & ~(0xf << 4)) | (DIVISOR_8 << 4)); //8 div
    io_raw_writel(tcfg1, TCFG1);
    vprintk("TCFG1:0x%x\n", io_raw_readl(TCFG1));
}
static void sched_clock_timer1_config(unsigned long cyc)
{
    io_raw_writew(cyc, TCNTB1);
    io_raw_writew(cyc, TCMPB1);
    vprintk("TCNTB1:%d\n", io_raw_readw(TCNTB1));
    vprintk("TCMPB1:%d\n", io_raw_readw(TCMPB1));
    return;
}
static void sched_clock_timer1_start(bool one_shot)
{
    uint32_t  tcon = 0;

    tcon = io_raw_readl(TCON);
    tcon |= (1 << 9);/* manual update of timer 1 */
    io_raw_writel(tcon, TCON);
    tcon = io_raw_readl(TCON);
    tcon = tcon & (~(0xf << 8));

    if (one_shot == true)
        tcon |= (0x1 << 8);
    else
        tcon |= (0x9 << 8);

    io_raw_writel(tcon, TCON);/* auto load, start timer */
    vprintk("TCON:0x%x\n", io_raw_readl(TCON));
    return;
}
static void sched_clock_timer1_stop(void)
{
    uint32_t  tcon = 0;

    tcon = io_raw_readl(TCON);
    tcon = tcon & (~(0x1 << 8));
    io_raw_writel(tcon, TCON);/* auto load, start timer */
    vprintk("TCON:0x%x\n", io_raw_readl(TCON));
    return;
}

static void timer1_irq(unsigned int irq)
{
    update_sched_clock();
}

static struct sched_clock_event clock_fops = {
    .read_cyc = sched_clock_read_timer0_cyc,
    .clock_config = sched_clock_timer1_config,
    .clock_start = sched_clock_timer1_start,
    .clock_stop = sched_clock_timer1_stop,
};
void sched_clock_timer_init(void)
{
    sched_clock_timer0_free_running();
    sched_clock_timer1_set_prescale_divisor();
    request_irq(IRQ_TIMER1, timer1_irq);
    sched_clock_register(&clock_fops, 16, 25000);
}
static void seos_timer_set_prescale_divisor(void)
{
    uint32_t tcfg0 = 0, tcfg1 = 0;

    tcfg0 = io_raw_readl(TCFG0);

    tcfg0 &= ~PRESCALER1_MASK;
    tcfg0 |= (249 << PRESCALER1_SHIFT); //prescaler 249 + 1 for timer2
    io_raw_writel(tcfg0, TCFG0);
    vprintk("TCFG0:0x%x\n", io_raw_readl(TCFG0));
    tcfg1 = io_raw_readl(TCFG1);
    tcfg1 &= ~TIMER2_DIVISOR_MASK;
    tcfg1 |= (DIVISOR_8 << TIMER2_DIVISOR_SHIFT); //8 div for timer2
    io_raw_writel(tcfg1, TCFG1);
    vprintk("TCFG1:0x%x\n", io_raw_readl(TCFG1));
}
static void seos_timer_config(unsigned long cyc)
{
    io_raw_writew(cyc, TCNTB2);
    io_raw_writew(cyc, TCMPB2);
    //vprintk("TCNTB2:%d\n", io_raw_readw(TCNTB2));
    //vprintk("TCMPB2:%d\n", io_raw_readw(TCMPB2));
    return;
}
static void seos_timer_start(bool one_shot)
{
    uint32_t  tcon = 0;

    tcon = io_raw_readl(TCON);

    tcon |= TCON_MANUALUPDATE(TIMER_2);
    io_raw_writel(tcon, TCON);
    tcon = io_raw_readl(TCON);
    tcon &= ~TCON_MANUALUPDATE(TIMER_2);
    tcon |= TCON_START(TIMER_2);

    if (!one_shot)
        tcon |= TCON_AUTORELOAD(TIMER_2);
    else
        tcon &= ~TCON_AUTORELOAD(TIMER_2);

    io_raw_writel(tcon, TCON);
    //vprintk("TCON:0x%x\n", io_raw_readl(TCON));
    return;
}
static void seos_timer_stop(void)
{
    uint32_t tcon = 0;

    tcon = io_raw_readl(TCON);
    tcon &= ~TCON_START(TIMER_2);
    io_raw_writel(tcon, TCON);/* auto load, start timer */
    //vprintk("TCON:0x%x\n", io_raw_readl(TCON));
    return;
}

static void seos_timer_irq(unsigned int irq)
{
    timIntHandler();
}
void seos_timer_init(void)
{
    seos_timer_set_prescale_divisor();
    request_irq(IRQ_TIMER2, seos_timer_irq);
}

/* seos timer is 40000ns one cyc */
bool platSleepClockRequest(uint64_t wakeupTime, uint32_t maxJitterPpm, uint32_t maxDriftPpm, uint32_t maxErrTotalPpm)
{
    uint64_t intState = 0, curTime = timGetTime();
    uint64_t time_diff = 0;
    uint32_t cyc = 0;
    uint64_t div_result = 0;

    if (wakeupTime && curTime >= wakeupTime)
        return false;

    div_result = wakeupTime - curTime;
    do_div(div_result, 40000);
    intState = cpuIntsOff();

    if (wakeupTime) {
        time_diff = wakeupTime - curTime;
        cyc = (div_result == 0) ? 1 : div_result;
        seos_timer_stop();
        seos_timer_config(cyc);
        seos_timer_start(true);
    }

    cpuIntsRestore(intState);
    return true;
}
